#include <navigation/rviz2_gui/rviz2_gui_panel.hpp>
#include <QAbstractItemView>
#include <QModelIndex>
#include <geometry_msgs/msg/pose.hpp>
#include <utility>
#include <QMenu>
#include <QAction>
#include <QFileDialog>
#include <QInputDialog>
#include <filesystem>
#include <std_srvs/srv/set_bool.hpp>
#include <atomic>

Rviz2GuiPanel::Rviz2GuiPanel(QWidget* parent)
    : rviz_common::Panel(parent) {
    QVBoxLayout* layout = new QVBoxLayout(this);
    layout->addWidget(new QLabel("Target List (Multi-select/Drag)", this));
    target_list_ = new QListWidget(this);
    target_list_->setSelectionMode(QAbstractItemView::ExtendedSelection);
    target_list_->setDragDropMode(QAbstractItemView::InternalMove);
    target_list_->setContextMenuPolicy(Qt::CustomContextMenu);
    layout->addWidget(target_list_);
    connect(target_list_->model(), &QAbstractItemModel::rowsMoved, this,
            &Rviz2GuiPanel::onListOrderChanged);
    connect(target_list_, &QListWidget::customContextMenuRequested, this, [this](const QPoint &pos) {
        QMenu menu;
        QAction* addOriginAct = menu.addAction("Add Origin Point");
        QList<QListWidgetItem*> selected = target_list_->selectedItems();
        QAction* delAct = nullptr;
        if (!selected.isEmpty()) {
            delAct = menu.addAction("Delete Target");
        }
        QAction* chosen = menu.exec(target_list_->viewport()->mapToGlobal(pos));
        if (chosen == addOriginAct) {
            geometry_msgs::msg::PoseStamped origin_pose;
            origin_pose.pose.position.x = 0.0;
            origin_pose.pose.position.y = 0.0;
            origin_pose.pose.position.z = 0.0;
            origin_pose.pose.orientation.x = 0.0;
            origin_pose.pose.orientation.y = 0.0;
            origin_pose.pose.orientation.z = 0.0;
            origin_pose.pose.orientation.w = 1.0;
            onMapClicked(origin_pose);
        } else if (delAct && chosen == delAct) {
            // 多选时，依次删除所有选中项（从大到小，防止索引错乱）
            QList<int> idxs;
            for (QListWidgetItem* item : selected) {
                idxs << target_list_->row(item);
            }
            std::sort(idxs.begin(), idxs.end(), std::greater<int>());
            for (int idx : idxs) {
                onRemoveTarget(idx + 1); // 保持和服务接口一致
            }
        }
    });

    // 在构造函数中添加信号槽，实时高亮选中目标
    connect(target_list_, &QListWidget::itemSelectionChanged, this, [this]() {
        QList<QListWidgetItem*> selected = target_list_->selectedItems();
        std::vector<int> indices;
        for (QListWidgetItem* item : selected) {
            indices.push_back(target_list_->row(item) + 1); // 保持和服务接口一致
        }
        auto req = std::make_shared<Rviz2Nav::Request>();
        req->type = static_cast<int>(Rviz2ConnecterType::HIGHLIGHT_TARGETS);
        req->highlighted_indices = indices;
        call_service_async(req);
    });

    // 按钮布局调整
    QHBoxLayout* btn_row = new QHBoxLayout();
    status_label_ = new QLabel(this); // 圆圈状态
    status_label_->setFixedSize(24, 24);
    status_label_->setStyleSheet("border-radius:12px;background:red;");
    btn_row->addWidget(status_label_);
    toggle_btn_ = new QPushButton("Start Move", this);
    btn_row->addWidget(toggle_btn_);
    adjuster_btn_ = new QPushButton("Pause Map Adjuster", this);
    btn_row->addWidget(adjuster_btn_);
    layout->addLayout(btn_row);
    connect(toggle_btn_, &QPushButton::clicked, this, [this]() {
        if (!client_) return;
        toggle_btn_->setEnabled(false); // 防止重复点击
        if (controller_is_paused_) {
            call_service(Rviz2ConnecterType::START);
            // 避免直接sleep导致界面卡顿，使用定时器单起线程延时
            QTimer::singleShot(500, this, [this](){ toggle_btn_->setEnabled(true); });  // twist publish sleep time 500ms
        } else {
            call_service(Rviz2ConnecterType::PAUSE);
            QTimer::singleShot(3000, this, [this](){ toggle_btn_->setEnabled(true); });  // twist publish sleep time 3000ms
        }
    });
    connect(adjuster_btn_, &QPushButton::clicked, this, [this]() {
        if (!adjuster_client_) return;
        auto req = std::make_shared<std_srvs::srv::SetBool::Request>();
        req->data = !adjuster_paused_ ? true : false; // 当前运行则暂停，当前暂停则启动
        adjuster_btn_->setEnabled(false); // 防止重复点击
        auto future = adjuster_client_->async_send_request(req,
            [this](rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture result) {
                adjuster_btn_->setEnabled(true);
                if (result.get()->success) {
                    adjuster_paused_ = result.get()->message.find("paused") != std::string::npos;
                    adjuster_btn_->setText(adjuster_paused_ ? "Start Map Adjuster" : "Pause Map Adjuster");
                } else {
                    QMessageBox::warning(this, "Map Adjuster", QString::fromStdString(result.get()->message));
                }
            }
        );
    });

    // Save/Load按钮
    save_btn_ = new QPushButton("Save", this);
    load_btn_ = new QPushButton("Load", this);
    QHBoxLayout* save_load_row = new QHBoxLayout();
    save_load_row->addWidget(save_btn_);
    save_load_row->addWidget(load_btn_);
    layout->addLayout(save_load_row);

    connect(save_btn_, &QPushButton::clicked, this, [this]() {
        bool ok = false;
        QString file_name = QInputDialog::getText(this, "Save Targets", "Enter file name:", QLineEdit::Normal, "targets", &ok);
        if (ok && !file_name.isEmpty()) {
            auto req = std::make_shared<Rviz2Nav::Request>();
            req->type = static_cast<int>(Rviz2ConnecterType::SAVE_TARGETS);
            req->message = file_name.toStdString() + ".txt";
            auto resp = call_service_async(req, true);
            if (resp.success) {
                QMessageBox::information(this, "Save Targets", QString::fromStdString(resp.message));
            } else {
                QMessageBox::warning(this, "Save Targets", QString::fromStdString(resp.message));
            }
        }
    });
    connect(load_btn_, &QPushButton::clicked, this, [this]() {
        QString dir = QString::fromStdString(maps_dir_path_);
        QString file_path = QFileDialog::getOpenFileName(this, "Load Targets", dir, "Text Files (*.txt);;All Files (*)");
        if (!file_path.isEmpty()) {
            auto req = std::make_shared<Rviz2Nav::Request>();
            req->type = static_cast<int>(Rviz2ConnecterType::LOAD_TARGETS);
            req->message = file_path.toStdString();
            call_service_async(req);
        }
    });

    update_timer_ = new QTimer(this);
    update_timer_->setInterval(500);    // 500ms刷新
    connect(update_timer_, &QTimer::timeout, this,
            &Rviz2GuiPanel::updateTargetList);
    update_timer_->start();

    // 初始化启动按钮和圆圈状态
    controller_is_paused_ = true;
    // 初始化adjuster按钮
    adjuster_paused_ = false;

    node_ = std::make_shared<rclcpp::Node>("rviz2_gui_panel_node");
    client_ = node_->create_client<Rviz2Nav>("rviz2_nav");
    adjuster_client_ = node_->create_client<std_srvs::srv::SetBool>("pause_map_adjuster");
    controller_is_paused_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
        "controller_is_paused", 1,
        [this](const std_msgs::msg::Bool::SharedPtr msg) {
            controller_is_paused_ = msg->data;
            // RCLCPP_INFO(node_->get_logger(), "Controller is paused: %s", controller_is_paused_ ? "true" : "false");
            toggle_btn_->setText(controller_is_paused_ ? "Start Move" : "Pause Move");
            status_label_->setStyleSheet(controller_is_paused_ ? "border-radius:12px;background:red;" : "border-radius:12px;background:green;");
        }
    );
    
    subscribe_target_names();
    subscribe_goal_pose();

    // 启动 spin 线程
    spin_thread_ = std::thread([this]() {
        rclcpp::spin(node_);
    });

    request_maps_dir_path();
}

Rviz2GuiPanel::~Rviz2GuiPanel() {
    if (node_) {
        rclcpp::shutdown();
    }
    if (spin_thread_.joinable()) {
        spin_thread_.join();
    }
}

void Rviz2GuiPanel::onInitialize() {}

void Rviz2GuiPanel::updateTargetList() {
    if (last_names_ != current_names_) {
        target_list_->clear();
        for (const auto& name : last_names_) {
            target_list_->addItem(QString::fromStdString(name));
        }
        current_names_ = last_names_;
    }
}

void Rviz2GuiPanel::onStartClicked() {
    call_service(Rviz2ConnecterType::START);
}
void Rviz2GuiPanel::onPauseClicked() {
    call_service(Rviz2ConnecterType::PAUSE);
}
void Rviz2GuiPanel::onListOrderChanged(const QModelIndex &parent, int start, int end, const QModelIndex &destination, int row) {
    (void)parent; (void)end; (void)destination;
    if (start == row || start+1 == row) return;
    auto req = std::make_shared<Rviz2Nav::Request>();
    req->type = static_cast<int>(Rviz2ConnecterType::MOVE_TARGET);
    req->target = geometry_msgs::msg::Pose();
    req->index1 = start + 1;
    req->index2 = ((row > start) ? row-1 : row) + 1;
    call_service_async(req);
}
void Rviz2GuiPanel::onMapClicked(const geometry_msgs::msg::PoseStamped& pose) {
    auto req = std::make_shared<Rviz2Nav::Request>();
    req->type = static_cast<int>(Rviz2ConnecterType::ADD_TARGET);
    req->target = pose.pose;
    call_service_async(req);
}
void Rviz2GuiPanel::onRemoveTarget(int index) {
    auto req = std::make_shared<Rviz2Nav::Request>();
    req->type = static_cast<int>(Rviz2ConnecterType::REMOVE_TARGET);
    req->index1 = index;
    call_service_async(req);
}
void Rviz2GuiPanel::call_service(Rviz2ConnecterType type, bool need_resp) {
    auto req = std::make_shared<Rviz2Nav::Request>();
    req->type = static_cast<int>(type);
    req->target = geometry_msgs::msg::Pose();
    call_service_async(req, need_resp);
}
Rviz2Nav::Response Rviz2GuiPanel::call_service_async(std::shared_ptr<Rviz2Nav::Request> req, bool need_resp) {
    if (!client_->wait_for_service(std::chrono::milliseconds(500))) {
        QMessageBox::warning(this, "Service Unavailable", "rviz2_nav service not available");
        Rviz2Nav::Response resp;
        resp.success = false;
        resp.message = "Service not available";
        return resp;
    }
    auto future = client_->async_send_request(req);
    if (need_resp) return *future.get();
    // 如果不需要响应，直接返回一个空的响应
    Rviz2Nav::Response resp;
    resp.success = true; // 假设调用成功
    resp.message = "Request sent successfully";
    return resp;
}
void Rviz2GuiPanel::subscribe_target_names() {
    names_sub_ = node_->create_subscription<navigation::msg::TargetsList>(
        "/target_names", 10,
        [this](const navigation::msg::TargetsList::SharedPtr msg) {
            last_names_ = msg->names;
        }
    );
}
void Rviz2GuiPanel::subscribe_goal_pose() {
    goal_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
        "/goal_pose", 10,
        [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
            RCLCPP_INFO(node_->get_logger(), "Start request");
            onMapClicked(*msg);
            RCLCPP_INFO(node_->get_logger(), "Received goal pose: [%f, %f, %f]",
                msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
        }
    );
}
void Rviz2GuiPanel::request_maps_dir_path() {
    auto req = std::make_shared<Rviz2Nav::Request>();
    req->type = static_cast<int>(Rviz2ConnecterType::GET_MAPS_DIR_PATH);
    auto resp = call_service_async(req, true);
    maps_dir_path_ = resp.message;
    RCLCPP_INFO(node_->get_logger(), "Received maps directory path: %s", maps_dir_path_.c_str());
}

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(Rviz2GuiPanel, rviz_common::Panel);
